#include "Arduino.h"
#include "HAL.h"
#include "Servo.h"
#include "SoftwareSerial.h"

static Servo VehicleSpeedServo;
static Servo EngineSpeedServo;
static Servo FuelLevelServo;
static SoftwareSerial SerialConnection(0,1);
static String recvCmd;
UInt16 HAL::timerCnt = 0;

//////////////////////////////////////////////////////////
//				interrupt routine: 10 ms				//
//////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect)
{
	HAL::timerCnt++;
}

void SetUpTimer(void)
{
	cli();			// disable global interrupts

	/****************/
	/* Set up pins	*/
	/****************/
	pinMode(LED_PIN, OUTPUT);			// LED for signaling current send bit
	
	for(Int8 i = 0; i < 6; i++)
	{
		digitalWrite(LED_PIN, ~digitalRead(LED_PIN));
		//digitalWrite(LED_PIN, HIGH);
		//digitalWrite(LED_PIN, LOW);
		delay(500);
	}
	
	
	
	/*****************/
	/* Set up Timer2 */
	/*****************/
	TCCR2A = 0; TCCR2B = 0;		// clear timer settings register

	// Aruino runs at 16 Mhz, so we need to have to get an interrupt after 1 ms
	// set compare match register to calculated timer count:
	OCR2A = INTERRUPT_CMP_CNT;

	// turn on CTC mode:
	TCCR2A |= (1 << WGM21);

	// Set CS20 and CS22 bits for 128 prescaler
	TCCR2B |= (1 << CS20);
	TCCR2B |= (1 << CS22);

	// activate timer compare interrupt for polling receive pin
	TIMSK2 |= (1 << OCIE2A);

	sei();		// enable global interrupts
}

void HAL::InitProject(void)
{
	/************************************/
	/* Initialization routine			*/
	/************************************/
	SetUpTimer();
	
	VehicleSpeedServo.attach(VEH_SPEED_PIN);
	VehicleSpeedServo.write(170);
	
	EngineSpeedServo.attach(ENG_SPEED_PIN);
	VehicleSpeedServo.write(180);
	
	FuelLevelServo.attach(FUEL_LVL_PIN);
	VehicleSpeedServo.write(180);
	
	SerialConnection.begin(9600);
	delay(5000);
	TRACE_NL("Start...");
	
	/* Arduino stuff*/
	init();

	#if defined(USBCON)
	USBDevice.attach();
	#endif

	String recvCmd;
	
	// while(true)
	//{
	//	digitalWrite(LED_PIN, ~digitalRead(LED_PIN));
	//}
}

UInt16 HAL::ReadAccelSensor(void)
{
	return analogRead(ACCEL_PIN);
}

UInt16 HAL::ReadBrakeSensor(void)
{
	return analogRead(BREAK_PIN);
}

void HAL::SetCarSpeedServo(UInt16 val)
{
	VehicleSpeedServo.write(val);
	digitalWrite(13, !digitalRead(13));
}

void HAL::SetEngineServo(UInt16 val)
{
	EngineSpeedServo.write(val);
	
	
}

void HAL::SetFuelServo(UInt16 val)
{
	FuelLevelServo.write(val);
}

void HAL::ProcessSerial()
{
	if (SerialConnection.available())
	{
		recvCmd = READ_STRING();
		if(recvCmd == "s")
		{
			TRACE("Phys soll speed: ");
			// TRACE(tempInt);
			TRACE(" Curr. speed: ");
			// TRACE(JanCar.GetVehicleSpeed());
			TRACE(" RPM: ");
			// TRACE_NL(JanCar.GetEngineSpeed());
		}
		else
		{
			// tempInt = recvCmd.toInt();
		}
		
		if (serialEventRun) serialEventRun();
	}
}